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Abstract-This  paper  is  concerned  with  tracking  of 
ground  targets  on  roads  and  investigates  possible  ways 
to  improve  target  state  estimation  via  fusing  a  target’s 
track  with  information  about  a  road  along  which  the 
target  is  believed  to  be  traveling.  A  target  track  is 
estimated  by  a  surveillance  radar  whereas  a  digital 
map  provides  the  road  network  of  a  region  under 
surveillance.  When  the  information  about  roads  is  as 
accurate  as  (or  even  better  than)  radar  measurements, 
it  is  desired  naturally  to  incorporate  such  information 
(fusion)  into  target  state  estimation.  In  this  paper, 
roads  are  modeled  with  analytic  functions  and  its 
fusion  with  a  target  track  is  cast  as  linear  or  nonlinear 
state  constraints  in  an  optimization  procedure.  The 
constrained  optimization  is  then  solved  with  the 
Lagrangian  multiplier,  leading  to  a  closed-form 
solution  for  linear  constraints  and  an  iterative  solution 
for  nonlinear  constraints.  Geometric  interpretations  of 
the  solutions  are  provided  for  simple  cases.  Computer 
simulation  results  are  presented  to  illustrate  the 
algorithms. 

Keywords:  Track  fusion.  Road  map.  State  constraints, 
Lagrangian  multiplier.  Iterative  solution 

1.  Introduction 

With  the  rapid  building  up  of  geographic  information 
system  (GIS)  including  digital  road  maps  (DRM)  and 
digital  terrain  elevation  database  (DTED),  information 
about  roads  becomes  more  accurate,  which  is  also  more 
up  to  date  and  easily  accessible.  Target  tracking  is  not 
unfamiliar  with  road  maps.  As  examples,  target  tracks 
are  represented  by  colorful  dots  and  lines  blinking 
along  road  networks  on  a  large  screen,  often  on  top  of  a 
topographic  or  satellite  image,  in  a  situation  room,  in 
an  air  traffic  control  tower,  and  on  a  radar  operator 
screen.  In  these  applications,  however,  target  tracks 
and  road  networks  are  merely  displayed  together  with 
little  or  no  interaction  (fusion)  in  the  data  processing 
level. 

When  the  information  about  roads  is  as  accurate  as 
(or  even  better  than)  radar  measurements,  it  is  desired 
naturally  to  incorporate  such  information  (fusion)  into 
target  state  estimation.  When  a  vehicle  travels  off-road 
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or  on  an  unknown  road,  the  state  estimation  problem  is 
unconstrained.  However,  when  the  vehicle  is  traveling 
on  a  known  road,  be  it  straight  or  curved,  the  state 
estimation  problem  can  be  cast  as  constrained  with  the 
road  network  information  available  from  digital 
road/terrain  maps.  In  the  past,  such  constraints  are 
often  ignored  (or  left  for  the  users  to  perceive  it  as  in 
the  display  example  mentioned  above).  The  resulting 
estimates,  even  obtained  with  the  Kalman  filter,  cannot 
be  optimal  because  they  do  not  make  full  use  of  this 
additional  information  about  state  constraints. 

To  use  such  state  constraints,  previous  attempts  can 
be  put  into  several  groups.  The  first  group  is  to 
incorporate  road  information  into  the  state  estimation 
process  [5,  6,  9,  11].  The  second  group  is  to  treat  state 
constraints  as  perfect  (pseudo)  measurements.  For  a 
road  segment,  its  analytic  model  not  only  constrains  the 
target  position  but  also  the  direction  of  the  target's 
velocity  vector.  Indeed,  the  target  velocity  vector  is 
closely  aligned  with  the  road  orientation  for  a  linear 
segment  and  with  the  tangent  vector  at  the  target 
position  for  a  nonlinear  segment.  Furthermore,  an 
estimate  of  centripetal  acceleration  can  be  obtained 
given  the  curvature  and  the  target  speed. 

In  the  third  group,  an  unconstrained  Kalman  filter 
solution  is  first  obtained  and  then  the  unconstrained 
state  estimate  is  projected  onto  the  constrained  surface 
[10].  This  technique  can  also  be  viewed  as  post¬ 
processing  (estimation  or  updating)  correction  [11]  or 
track  to  road  fusion  referred  to  as  in  this  paper.  In 
conventional  track  fusion,  two  or  more  tracks  are 
available,  each  consisting  of  an  estimate  of  the 
underlying  track  with  its  estimation  error  covariance. 
The  fused  track  is  typically  found  that  minimizes  the 
sum  of  covariance-weighted  state  errors  squared  [3,  4]. 
In  contrast  to  this  conventional  track  fusion  that 
operates  on  individual  state  values  (points),  track 
fusion  with  road  involves  a  state  value  (a  point)  and  a 
subset  of  state  values  (an  interval).  In  this  paper,  roads 
are  modeled  with  analytic  functions  and  its  fusion  with 
a  target  track  is  therefore  formulated  as  linear  or 
nonlinear  state  constraints  in  an  optimization 
procedure. 
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In  this  paper,  the  constrained  optimization  is  solved 
with  the  Lagrangian  multiplier,  leading  to  a  closed- 
form  solution  for  linear  constraints  and  an  iterative 
solution  for  nonlinear  constraints.  In  the  latter  case,  we 
present  a  method  that  allows  for  the  use  of  second- 
order  nonlinear  state  constraints  exactly.  The  method 
can  provide  better  approximation  to  higher  order 
nonlinearities.  The  new  method  is  based  on  a 
computational  algorithm  that  iteratively  finds  the 
Lagrangian  multiplier.  The  use  of  a  second-order 
constraint  vs.  linearization  is  a  tradeoff  between 
reducing  approximation  errors  to  higher-order 
nonlinearities  and  keeping  the  problem 
computationally  tractable. 

Although  the  main  results  are  restricted  to  state 
equality  constraints,  it  can  be  extended  to  inequality 
constraints.  According  to  [10],  the  inequality 
constraints  can  be  checked  at  each  time  step  of 
filtering.  If  the  inequality  constraints  are  satisfied  at  a 
given  time  step,  no  action  is  taken  since  the  inequality 
constrained  problem  is  solved.  If  the  inequality 
constraints  are  not  satisfied  at  a  given  time  step,  then 
the  constrained  solution  is  applied  to  enforce  the 
constraints. 

2.  Track  Fusion  with  Linear  Road  Segment 

When  a  road  segment  is  straight,  it  can  be  modeled  as  a 
linear  state  constraint.  In  this  section,  we  first 
summarize  the  results  for  linearly  constrained  state 
estimation  [10]  as  an  approach  to  track  fusion  with 
linear  road  segments.  We  then  show  that  this  linearly 
constrained  state  estimation  is  equivalent  to  use  of 
constraints  as  measurements  in  state  update.  Finally, 
we  provide  a  simple  geometric  interpretation  of  the 
linearly  constrained  state  estimation  for  track  to  road 
fusion. 

Consider  a  linear  time-invariant  discrete-time 
dynamic  system  together  with  its  measurement  as 

xk+l=Axk+Buk+wk  (la) 

yk=Cxk+v_k  (lb) 

where  the  underscore  indicates  a  vector  quantity,  the 
subscript  k  is  the  time  index,  x  is  the  state  vector,  u  is  a 
known  input,  y  is  the  measurement,  and  yy  and  v  are 
state  and  measurement  noise  processes,  respectively.  It 
is  implied  that  all  vectors  and  matrices  have  compatible 
dimensions,  which  are  omitted  for  simplicity. 

The  goal  is  to  find  an  estimate  denoted  by  xt  of  xk 

given  the  measurements  up  to  time  k  denoted  by  Yk  = 
[vo,  ....  V/ } .  Under  the  assumptions  that  the  state  and 
measurement  noises  are  uncorrelated  zero-mean  white 
Gaussian  with  yv  ~  'N\ 0,  Q }  and  y  ~  'N\0,  K)  where  Q 
and  R  are  positive  semi-definite  covariance  matrices, 


the  Kalman  filter  provides  an  optimal  estimator  in  the 
form  of  X:  =E{xk  Yk )  [2].  Starting  from  an  initial 

estimate  L  =  E{x0]  and  its  estimation  error  covariance 
matrix  PQ  =  E{{x0  -*„)(*„  -x0)r]  where  the 

superscript  T  stands  for  matrix  transpose,  the  Kalman 
filter  equations  specify  the  propagation  of  x  t  and  Pk 

over  time  and  the  update  of  L  and  Pk  by  measurement 


La- as 

lk+]  =  Axk  +  Buk 

(2a) 

Pt+i=APtAT+Q 

(2b) 

xk+l  =  Il+1  +  Kk+1(yk+i  -  Cxk+l) 

(2c) 

PM=d-Kk+lC)PM 

(2d) 

KkM=PkMCT(CPkCT  +R)-' 

(2e) 

where  v  ,  and  p  are  the  predicted 

state  and 

prediction  error  covariance,  respectively. 

Now  in  addition  to  the  dynamic  system  of  (1),  we  are 
given  the  linear  state  constraint  equation 

Dxk  =  d  (3) 

where  D  is  a  known  constant  matrix  of  full  rank,  d  is  a 
known  vector,  and  the  number  of  rows  in  D  is  the 
number  of  constraints,  which  is  assumed  to  be  less  than 
the  number  of  states.  If  D  is  a  square  matrix,  the  state  is 
fully  constrained  and  can  thus  be  solved  by  inverting 
(3).  Although  no  time  index  is  given  to  D  and  d  in  (3), 
it  is  implied  that  they  can  be  time-dependent,  leading  to 
piecewise  linear  constraints. 

The  constrained  Kalman  filter  according  to  [10]  is 
constructed  by  directly  projecting  the  unconstrained 
state  estimate  x,  onto  the  constrained  surface  5  =  [x: 
Dx  =  d).  It  is  formulated  as  the  solution  to  the  problem 
x  =  arg  min(x  -  x) 1  W{  x  -  x)  (4) 

X<E  S 

where  W  is  a  symmetric  positive  definite  weighting 
matrix. 

Derived  using  the  Lagrangian  multiplier  technique 
[16],  the  solution  to  the  constrained  optimization  in  (4) 
is  given  by 

x=x-W~1DT(DW-'DT)-1(Dx-d)  (5) 

As  described  above,  the  linear  constrained  estimator 
(5)  can  be  obtained  by  different  methods.  It  is  shown  in 
this  section  that  it  is  also  equivalent  to  the  solution 
where  the  linear  state  constraints  are  considered  as 
perfect  (pseudo)  measurements. 

For  the  linear  time-invariant  discrete-time  dynamic 
system  (la)  and  its  measurement  (lb),  consider  the 
linear  state  constraint  (3)  as  another  measurement  to 
the  system,  which  can  be  used  to  perform  the  filter 


measurement  update  (2c)  and  (2d)  right  after  (lb) 
without  the  filter  time  propagation  (2a)  and  (2b)  (i.e., 
stay  the  same).  To  apply  (2),  we  identify  the  following 
equivalence: 


A  =  I,  B  =  0  ;  Q  =  0 

(6a) 

C  =  D,  R  =  0,  =  d 

(6b) 

Given  the  unconstrained  solution 

(xk,Pk),  the 

prediction  step  is  given  by  (2a)  and  (2b): 

£t-+i  i 

(7a) 

P  =p 

rk+l  k 

(7b) 

The  Kalman  filter  gain  is  given  by: 

Kk+i=PkDr(DPkDTy' 

(8) 

The  updated  state  and  error  covariance  becomes: 

xk+l  =xk  +  PkDT{DPkDTY\d-Dxk) 

(9a) 

PM  =  Pk~PkDT(DPkDTy'DPk 

(9b) 

If  we  choose  w  =  Pd' ,  (9a)  becomes 

xk+l  =  xk  +  W  'D1  ( DW  ~'Dr  y1  (d  -  Dxk ) 

(10a) 

=  xk -W'DT(DW~'DTy\Dxk -d) 

(10b) 

which  is  exactly  the  same  as  the  solution  given  by  (5). 

Assume  that  the  state  dimension  is  n  and  the  number 
of  linear  constraints  is  m  <  n.  For  x  e  R",  the  constraint 
surface  S  =  {x:  Dx  =  d)  is  not  a  subspace  simply 
because  for  d  ^  0,  the  null  vector  is  not  inside  S. 

To  construct  a  subspace,  first  find  an  arbitrary  point 
x()  g  5  and  then  define  £=  x  -  Xq.  This  is  equivalent  to 
shifting  the  origin  of  the  coordinates  to  x0,  thus 
performing  an  affine  transformation  denoted  by  T.  For 
all  x  e  S,  the  corresponding  shifted  vector  g  has  the 
following  property: 

D£=  D(x  -  Xq )  =  Dx  -  Dxq  =  d-  d  =  0  (11) 

In  other  words,  the  constraint  surface  after  the  affine 
transformation  T  becomes  a  subspace,  denoted  by  £  = 
Ts  =  {g  Dg  =  0),  which  has  a  dimension  n-m.  The 
affine  transformation  is  illustrated  in  Fig.  1 . 

We  are  now  to  express  £.  But  first,  the  row  vectors 
of  D  can  be  expressed  as: 

DT=\di  d2  ...  dj  (12) 

Since  D  is  of  full  rank  by  assumption,  the  row 
vectors  of  D  can  be  used  as  the  non-orthogonal  bases 
for  a  subspace  denoted  by  D  =  span{Jlf  d2,  ...,  d,„  1  •  hi 
light  of  (11)  and  by  definition  of  £,  it  is  easy  to  see  that 
T>  is  an  orthogonal  complement  of  £,  that  is,  2?_L£  and 
£>©£  =  Rn. 


S=YJcidi=\dl  d2 


=  D  c 


(13) 


Then  for  ge  £,  we  have 

<S,l>=<DTc,l>=STl  =  cTDl=  0  (14) 

where  <a,  b>  =  a  h  is  the  inner  product  defined  on  R". 

By  the  principle  of  orthogonality,  an  arbitrary  vector 
£  can  be  decomposed  into  its  projections  onto  the 
orthogonal  complements  D  and  £,  denoted  by  £/>  and 
respectively,  as 

#  =  #  +#  (15) 

2.  Z -D  5 .L 

Adding  Xo  to  both  sides  of  (15),  we  can  express  the 
vectors  in  the  original  coordinates  as: 

x  =  l  +  x0=lD+lL  +  x0  =  lD+x  (16) 

The  projection  of  the  arbitrary  vector  on  the 
constraint  subspace  £  and  the  constraint  surface  S  can 
be  obtained,  respectively,  as: 

£  =£-£  (17a) 

2-i  2  2  D 

x  =  x  -  £  (1 7b) 

To  obtain  §p,  express  it  as  a  linear  combination  of 
the  non-orthogonal  bases  of  DT  with  the  coefficient 
vector  c  as: 


lD=Hc,d,=\d- ,  d2  •••  dm 


(18) 


Again,  by  the  principle  of  orthogonality,  the 
projection  error  vector  g-  Q)  is  orthogonal  to  £>,  i.e., 
each  an  every  basis  of  it: 

4,  >=<l~DTc,di  >=  d* il~  DT c)  =  0, 

i=\,...,m  (19) 

Stacking  these  orthogonality  conditions,  we  obtain 

d\ 


d‘ 


(£-  DT c)  =  0 


or  D(i_-  D  c)  =  0 


(20) 


Since  DD1  is  an  mxm  matrix  and  invertible,  the 

(21) 


coefficient  vector  can  be  obtained  as: 
c  =  {DDt)~'D 4 


For  Se  T>,  it  can  be  written  as: 


Bringing  (21)  back  to  (18)  gives  the  projection 
vector  as: 


Id=Dt{DDt)~'DI  =  PI  (22) 

where  P  =  Dt{DDt)~'D  is  usually  referred  to  as  the 
projection  matrix  onto  V  and  (I-P)  is  the  projection 
matrix  onto  £. 

Bringing  (22)  back  to  ( 17)  gives 
|;=|-P|=(/-P)|  (23a) 

x  =  x-  P%=  x-  P(x-x0)  (23b) 

Bringing  the  expression  for  P  into  (23b)  gives 
/  =  x—  Dt  (DDt  )_1  D(x  —  x0) 

=  x-DT(DDT)~1  (Dx-Dx0) 

=  x-DT(DDT)~1(Dx-d)  (24) 

where  Dx0  =  d  is  used  to  arrive  at  the  last  equation 
because  of  x0  e  S. 

(24)  is  exactly  the  same  as  (5)  when  W  =  I.  This 
offers  a  geometrical  interpretation  that  the  linear 
constrained  estimation  is  the  orthogonal  projection  of 
the  unconstrained  estimate  onto  the  constrained 
surface.  It  provides  a  theoretical  justification  of  the 
intuitive  practice  of  finding  a  point  along  the  road  that 
is  of  the  shortest  distance. 

The  theory  still  holds  for  W  ^  1.  The  results 
presented  in  this  paper  complement  [10],  providing  an 
interesting  geometrical  interpretation  to  the  linear 
constrained  estimation  by  estimate  projection. 

3.  Track  Fusion  with  Nonlinear  Road  Segments 

When  a  road  segment  is  curved,  it  can  be  modeled  as  a 
nonlinear  state  constraint.  In  this  section,  we  first 
analyze  the  linearizing  approach  and  the  associated 
constraint  approximation  error.  We  then  present  an 
iterative  solution  to  a  second  order  state  constraint. 
Finally,  we  offer  a  geometric  interpretation  of  a 
solution  under  a  circular  constraint  and  a  simple 
approach  to  a  more  general  second  order  state 
constraint. 

To  deal  with  nonlinearity,  a  simple  approach  is  to 
project  the  unconstrained  state  estimate  onto  linearized 
state  constraints.  Once  the  constraints  are  linearized, 
the  results  presented  in  the  previous  section  for  linear 
cases  can  be  applied.  However,  linearization  introduces 
constraint  approximation  error,  which  is  a  function  of 
the  nonlinearity  and.  more  importantly,  of  the  point 
around  which  the  linearization  takes  place.  This  may 
lead  to  an  undesired  divergence  problem  as  analyzed 
below. 

Fig.  2  illustrates  this  linearization  process  and 
identifies  possible  errors  associated  with  linear 
approximation  of  a  nonlinear  state  constraint.  As 
shown,  the  previous  constrained  state  estimate  x  lies 


somewhere  on  the  constrained  surface  but  is  away  from 
the  true  state  x.  The  projection  of  the  unconstrained 
state  estimate  x  onto  the  approximate  linear  state 
constraint  produces  the  current  constrained  state 
estimate  i+,  which  is  however  subject  to  the  constraint 
approximation  error.  Clearly,  the  further  away  is  x~ 
from  x,  the  larger  is  the  approximation-introduced 
error.  More  critically,  such  an  approximately  linear 
constrained  estimate  may  not  satisfy  the  original 
nonlinear  constraint.  It  is  therefore  desired  to  reduce 
this  approximation-introduced  error  by  including 
higher-order  terms  while  keeping  the  problem 
computationally  tractable.  One  possible  approach  is 
presented  in  the  next  section. 

Naturally  formed  roads  tend  to  have  more  bends  and 
turns  of  irregular  shapes  (high  nonlinearity).  Even 
highways  have  to  follow  terrain  contours  when 
crossing  mountains.  Locally,  however,  it  suffices  to 
represent  a  curved  road  segment  by  a  second-order 
state  constraint  function  as 


[  T  !  1 

~M 

m 

X 

[x  lj 

T 

m 

m0_ 

1 

=  xT  M  x+ mT  x+ xT  m+ m0  =0  (26) 

which  can  be  viewed  as  a  second-order  approximation 
to  an  arbitrary  nonlinearity  in  a  digital  terrain  map. 

Similar  to  (4),  we  can  formulate  the  projection  of  an 
unconstrained  state  estimation  onto  a  nonlinear 
constraint  surface  as  the  constrained  least-square 
optimization  problem 

x=argmin  (z- H  x)T  (z- H  x)  (27a) 

x 

subject  to  f(x)  =  0  (27b) 

If  we  let  W  =  HtH  and  z  =  Hx,  the  formulation  in 
(27)  becomes  the  same  as  in  (4).  In  a  sense,  (27)  is  a 
more  general  formulation  because  it  can  also  be 
interpreted  as  a  nonlinear  constrained  measurement 
update  or  a  projection  in  the  predicted  measurement 
domain. 


The  solution  to  the  constrained  optimization  (27)  can 
be  obtained  again  using  the  Lagrangian  multiplier 
technique  as  [12] 


x  =  G~lV(I  +  ALT'Lyle{A) 
q(A) 


e;(A)a f 
i  (\  +  Aaf  )2 


ri +A(j, 


2  +»v 


(28a) 

(28b) 


where  G  is  an  upper  right  diagonal  matrix  resulting 
from  the  Cholesky  factorization  of  W  =  HTH,  V,  an 
orthonormal  matrix,  and  2j  a  diagonal  matrix  with  its 
diagonal  elements  denoted  by  0[ ,  are  obtained  from  the 
singular  value  decomposition  of  the  matrix  LG"1,  and 


(35) 


e(A)  =  [...  e,(A),  ...]T=  Vt(GtT,(Htz  -  Am)  (29c) 
t  =  [...ti...]T=VT(GTr,m  (29d) 

As  a  nonlinear  equation  in  A,  it  is  difficult  to  find  a 
closed-form  solution  in  general  for  the  nonlinear 
equation  q(A)  =  0  in  (28b).  Numerical  root-finding 
algorithms  may  be  used  instead.  For  example,  the 
Newton's  method  is  used  below.  Denote  the  derivative 
of  q(A)  with  respect  to  k  as  q(  A)  •  Then  the  iterative 
solution  for  X  i  s  given  by 

i  —  x  _  c!^k) ,  starting  with  Ao  =  0  (30) 

“  ‘  q{Ak) 

Now  consider  the  special  case  where  W  =  HTH,  z  = 
Hx ,  and  m  =  0,  that  is,  a  quadratic  constraint  on  the 
state.  Under  these  conditions,  t  =  0  and  e  is  no  longer  a 
function  of  A  so  its  derivative  relative  to  A  vanishes. 
The  quadratic  constrained  solution  is  then  given  by 

x=(W+AM)'1Wx  (31a) 

where  the  Lagrangian  multiplier  A  is  obtained 
iteratively  as  in  (30)  with  the  corresponding  q(A)  and 
q{A)  given  by 

<3lc) 

The  solution  of  (31)  is  also  called  the  constrained 
least  squares  [8:  pp  765-766],  which  was  previously 
applied  for  the  joint  estimation  and  calibration  [13]. 
Similar  techniques  have  been  used  for  the  design  of 
filters  for  radar  applications  [1]  and  in  robust  minimum 
variance  beamforming  [7].  When  M  =  0,  the  constraint 
in  (26)  degenerates  to  a  linear  one.  The  constrained 
solution  is  still  valid.  However,  the  iterative  solution 
for  finding  A  is  no  longer  applicable  but  a  closed-form 
solution  is  available  instead  as  given  in  (5). 

Consider  a  simple  example  where  a  target  travels 
along  a  circle.  For  this  case,  in  fact,  a  closed-form 
solution  can  be  derived.  Assume  that  W  =  T,  M  =  I2,  m 
=  0,  and  nig  =  -r2.  The  nonlinear  constraint  can  be 
equivalently  written  as: 

xTx=r  (32) 

The  quadratic  constrained  estimate  given  in  (31a) 
becomes: 

x  =  {W  +  AM)-lWx  =  {\  +  Ay'x  (33) 

Bringing  (35)  back  to  (34)  gives: 

xT x  =  (—^—)T  =  r2  (34) 

-  -  1 +  A  1 +  A 

The  solution  for  A  is: 


r  r 

where  iui  stands  for  the  2-norm  of  the  vector. 

II  lb 

Bringing  the  solution  for  A  in  (35)  back  to  (33)  gives: 


This  indicates  that  for  this  particular  case  with  a 
circular  constraint,  the  constraining  results  in 
normalization.  This  further  suggests  a  simple  solution 
for  some  practical  applications.  When  a  target  is 
traveling  along  a  circular'  path  (or  approximately  so), 
one  can  first  find  the  equivalent  center  of  the  circle 
around  which  to  establish  a  new  coordinate  system. 
Then  express  the  unconstrained  solution  in  the  new 
coordinate  and  normalize  it  as  the  constrained  solution. 
Finally  convert  it  back  to  the  original  coordinates. 

4.  Simulation  Results 

In  this  simulation  example,  a  ground  vehicle  is 
assumed  to  travel  along  a  circular  road  segment  as 
shown  in  Fig.  2.  The  turn  center  is  chosen  as  the  origin 
of  the  x-y  coordinates  and  the  turn  radius  is  r  =  100  m. 
The  target  maintains  a  constant  turn  rate  of  5.7  deg/s 
with  an  equivalent  linear  speed  of  10  m/s.  The  initial 
state  is 

i=o  =  ix  *  y  vf  -  [100  m,  0  m/s,  0  m  10  m/s]1  (37) 

The  vehicle  is  tracked  by  a  radar  sensor  with  a 
sampling  interval  of  T  =  1  s.  The  sensor  provides 
position  measurements  of  the  vehicle  as 

1  0  0  0  /"5Q\ 

V  =  xk  +  v.  F’5/ 

-*  |_°  0  1  °J 

where  the  measurement  error  y  ~  W(0,  R)  is  a  zero- 
mean  Gaussian  noise,  independent  in  the  x-  and  y-axis. 
The  covariance  matrix  R  =  diag (\<Jrr~  <J,G\)  uses  the 
particular  values  of  arx  =  <Jry  =  7  m  in  the  simulation. 

The  radar  implements  a  simple  tracker  based  on  the 
following  discrete-time  second-order  kinematic  model 


0  10  0 
0  0  1  T 

0  0  0  1 


,  •>  wk 

0  \T-  -1 


where  the  process  noise  w  ~  W( 0,  Q )  is  also  a  zero- 
mean  Gaussian  noise,  independent  of  the  measurement 
noise  y.  The  covariance  matrix  Q  =  diag([  a2  „  ])  uses 

the  particular  values  of  a  =  0  =  0.32  m/s2  in  the 
simulation. 


When  represented  in  a  Cartesian  coordinate  system, 
a  target  traveling  along  a  curved  road  is  certainly 
subject  to  acceleration  in  both  the  x-  and  y-axis. 
However,  no  effect  is  made  in  this  simulation  to 
optimize  the  tracker  for  maneuver  but  merely  to  select 
Q  and  the  initial  conditions  so  as  to  focus  on 
constraining  the  estimates.  The  initial  state  is  selected 
to  be  the  same  as  the  true  state,  i.e.,  y  =  Y  and  the 

initial  estimation  error  covariance  is  selected  to  be 
P0  =  diag([52  l2  52  l2]) 

Fig.  3  shows  sample  trajectories  of  the  linear 
constrained  Kalman  filter.  There  are  5  curves  and  2 
series  of  data  points  in  the  figure.  The  true  state  is 
represented  by  a  series  of  dots  (•)  at  consecutive 
sampling  instants,  which  is  plotted  on  the  solid  line 
being  the  road  segment.  The  corresponding 
measurements  are  a  series  of  circles  (o). 

The  estimates  of  the  unconstrained  Kalman  filter  are 
shown  as  the  connected  triangles  (A)  whereas  those  of 
linearly  constrained  Kalman  filters  are  shown  as  the 
connected  crosses  (x),  stars  (*),  and  pluses  (+)  for  three 
linear  approximations  of  the  nonlinear  constraint  of 
curved  road,  respectively. 

In  the  first  approximation  (the  line  with  cross  X 
labeled  “linear  constraint  1”),  a  single  linearizing  point 
at  0\  =  10°  is  chosen  to  cover  the  entire  curved  road, 
where  0  is  the  angle  made  relative  to  the  x-axis, 
positive  in  the  counter-clock  direction.  The  linearized 
state  constraint  at  ft  can  be  written  as 

cos#,  0  sin#,  0 
0  cos#!  0  sin#! 

Although  all  estimates  are  faithfully  projected  by  the 
constrained  filter  onto  this  linear  constraint,  tangential 
to  the  curve  at  the  linearizing  point,  it  runs  away  from 
the  true  trajectory  and  the  resulting  errors  continue  to 
grow.  The  apparent  divergence  is  caused  by  the  choice 
of  linearization. 

In  the  second  approximation  (the  line  with  star  * 
labeled  “linear  constraint  2”),  two  linearizing  points  at 
#i  =  15°  and  ft  =  80°  are  chosen  to  cover  the  curved 
road  with  two  linear  segments.  The  switching  point 
from  one  linear  segment  to  the  other  in  this  case  is  at  # 
=  45°.  As  shown,  the  estimates  are  projected  onto  one 
of  the  two  linear  segments.  Except  near  the  corner 
where  the  two  linear  approximations  intersect  (which  is 
far  away  from  both  linearizing  points),  the  linear 
constrained  estimates  typically  outperform  the 
unconstrained  estimates  (closer  to  the  true  state).  This 
is  better  illustrated  in  Fig.  4  where  the  upper  plot  is  for 
the  absolute  position  error  in  x  while  the  lower  plot  is 
for  the  absolute  position  error  in  y,  both  plotted  as  a 
function  of  time. 


Still  with  two  linearizing  points  and  the  same 
switching  point  at  #=  45°,  the  third  approximation  (the 
line  with  star  +  labeled  “linear  constraint  3”)  adjusts 
linearizing  points  to  ft  =  20°  and  ft  =  70°.  A  better 
overall  performance  is  achieved  as  shown  in  Fig.  4. 

It  is  clear  from  Fig.  3  that  a  nonlinear  constraint  can 
be  approximated  with  linear  constraints  in  a  piecewise 
fashion.  By  judicious  selection  of  the  number  of  linear 
segments  and  their  placement  (i.e.,  the  point  around 
which  to  linearize),  a  reasonably  good  pciTornyiqrp  can 
be  expected.  In  the  limit,  a  nonlinear  function  is 
represented  by  a  piecewise  function  composed  of  an 
infinite  number  of  linear  segments.  This  naturally  leads 
to  the  use  of  nonlinear  constraints. 


Fig.  5  shows  sample  trajectories  of  the  nonlinear 
constrained  Kalman  filter.  There  are  2  curves  and  4 
series  of  data  points  in  the  figure.  The  true  state  is  still 
represented  by  a  series  of  dots  (•)  at  the  sampling 
instants,  which  is  plotted  on  the  solid  line  of  road 
segment.  The  corresponding  measurements  are  again  a 
series  of  circles  (o).  The  unconstrained  Kalman  filter  is 
shown  as  the  connected  crosses  (X)  whereas  the 
estimates  of  nonlinearly  constrained  Kalman  filters  are 
shown  as  a  series  of  connected  pluses  (+)  and  stars  (*) 
for  two  implementations,  respectively. 

The  first  implementation  (the  series  of  pluses  +)  only 
applies  the  nonlinear  constraint  to  the  position  estimate 
whereas  the  second  implementation  (the  series  of  stars 
*)  applies  constraints  to  both  the  position  and  velocity 
estimates.  In  fact,  we  encounter  a  hybrid  (mixed)  linear 
and  nonlinear  state  constraint  situation.  The 
constrained  position  estimate  is  given  by  (31)  for  the 
quadratic  case  (equivalent  to  either  (36)  for  a  circular 
road).  Since  the  velocity  direction  is  along  the  tangent 
of  the  road  curve,  the  constrained  velocity  estimate  is 
obtained  by  the  following  projection 


—constrained 


=  (v 


unconstrained 


M)M 


(41) 


where  f  =  [x  vf  is  the  estimated  velocity  vector  and  // 

=  [-sin (9  cos0\l  is  the  constrained  unit  direction  vector 
associated  with  the  constrained  position  at 
0  =  tan  '(y  lx)  ■ 


For  simplicity,  the  unconstrained  estimation  error 
covariance  is  not  modified  in  the  present  simulation 
after  the  constrained  estimate  is  obtained  using  the 
projection  algorithms  in  (31)  and  (41).  The 
implementation  is  therefore  pessimistic  (suboptimal)  in 
the  sense  that  it  does  not  take  into  account  the 
reduction  in  the  estimation  error  covariance  brought  in 
by  constraining.  One  consequence  of  this  simplification 
is  more  volatile  state  estimates.  To  quantify  this  effect, 
one  approach  is  to  project  the  unconstrained  probability 
density  function  (i.e.,  a  normal  distribution  with 


X  True  State  X  Previous  Constrained  State  Estimate 

X  Unconstrained  State  Estimate  x+  Current  Constrained  Sate  Estimate 


Fig.  2.  Errors  in  Linear  Approximation  of  Nonlinear  State  Constraints 


nonlinear  constrained  tracking  solution 


linear  constrained  position  absolute  errors 


Fig.  3.  Sample  Trajectories  for  Linear  Constrained  Kalman  Filter 


Fig.  4.  Linear  Constrained  Position  Errors  vs.  Time 


nonlinear  constrained  tracking  solution 


nonlinear  constrained  position  absolute  errors 


Fig.  5.  Sample  Trajectories  for  Nonlinear  Constrained  Kalman  Filter 


Fig.  6.  Nonlinear  Constrained  Position  Errors  vs.  Time 


support  on  the  whole  state  space)  onto  the  nonlinear 
constraint.  Statistics  can  then  be  calculated  from  the 
constrained  probability  density  function  with  the 
constraint  as  its  support.  Again,  the  resulting  error 
ellipse  represented  by  the  covariance  matrix  is  only  an 
approximation  to  the  second  order. 

As  shown  in  Fig.  5,  both  the  nonlinear  constrained 
estimates  fall  onto  the  road  as  expected.  Overall  the 
position  and  velocity  constrained  estimates  are  better 
(closer  to  the  true  state)  than  the  position-only 
constrained  estimates.  This  is  illustrated  in  Fig.  6 
where  the  upper  plot  is  for  the  absolute  position  error 
in  x  while  the  lower  plot  is  for  the  absolute  position 
error  in  y. 

A  Monte  Carlo  simulation  is  used  to  generate  the 
RMS  errors  of  state  estimation.  The  results  are  based 
on  a  total  of  100  runs  across  16  updates  and 
summarized  in  Table  1.  The  performance  improvement 
of  the  nonlinear  constrained  filter  over  the  linearized 
constrained  filter  is  demonstrated. 


Table  1.  RMS  Estimation  Errors 


Estimators 

RMS  Estimation  Error 

Position  (m) 

Velocity  (m/s) 

Unconstrained 

8.3663 

4.2640 

Best  Linear 
Constrained 

5.5386 

2.5466 

Nonlinear 

Constrained 

1.8056 

0.4252 

5.  Conclusions 

In  this  paper,  we  presented  an  approach  to 
incorporating  road  information  into  target  tracking  via 
track  to  road  fusion.  In  this  approach,  road  segments 
were  modeled  with  analytic  functions  and  their  fusion 
with  a  target  track  was  cast  as  a  linearly  or  nonlinearly 
state  constrained  optimization  procedure.  With  the 
Lagrangian  multiplier,  a  closed-form  solution  was 
found  for  linear  constraints  and  an  iterative  solution  for 
nonlinear  constraints.  Geometric  interpretations  of  the 
solutions  were  provided  for  simple  cases.  Computer 
simulation  results  demonstrate  the  performance  of  the 
algorithms. 

Future  work  includes  both  algorithms  development 
and  practical  applications.  It  is  of  interest  to  extend  the 
iterative  method  presented  in  the  paper  for  second- 
order  nonlinear  state  constraints  to  other  types  of 
nonlinear  constraints  of  practical  significance  and  to 
search  for  more  efficient  root-finding  algorithms  to 
solve  for  the  Lagrangian  multiplier.  Similarly,  the 
simple  fusion  of  a  single  track  to  a  single  road  as 
presented  in  this  paper  is  being  extended  to  multiple 
targets  moving  along  closely-spaced  road  networks 


with  intersections  and  by-passes.  In  this  case,  the 
fusion  (or  constraining)  can  take  place  in  the 
measurement  level  as  well  as  in  the  track  level, 
involving  road  constrained  data  association  (RCDA). 
Results  will  be  reported  in  future  papers. 
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